/**
 ******************************************************************************
 * @file    drv_packer.c
 * @author  zpw
 * @version V1.0
 * @date
 * @brief   链路层通讯协议
 ******************************************************************************
 * @attention
 *          链路层通讯协议，数据封包解包
 ******************************************************************************
 */
#include "upacker.h"

using namespace std;

/**
 * @brief  解包输入数据
 * @note
 * @param  cmd_packer:
 * @param  *buff:
 * @param  size:
 * @retval None
 */
void Upacker::upacker_unpack(QByteArray buff) {
  for (uint16_t i = 0; i < buff.size(); i++) {
    if (frame_decode(buff.at(i))) {
      //解析成功,回调处理
      emit recvPack(QByteArray((const char *)this->data, this->flen));
    }
  }
}

uint8_t Upacker::frame_decode(uint8_t d) {
  if (this->state == 0 && d == STX_L) {
    this->state = 1;
    this->calc = STX_L;
  } else if (this->state == 1) {
    this->flen = d;
    this->calc ^= d;
    this->state = 2;
  } else if (this->state == 2) {
    //长度信息
    this->flen |= (uint16_t)d << 8;
    this->calc ^= d & 0x3F;

    //数据包超长得情况下直接丢包
    if ((this->flen & 0x3FFF) > MAX_PACK_SIZE) {
      this->state = 0;
    }
    this->state = 3;
    this->cnt = 0;
  } else if (this->state == 3) {
    // header校验
    uint8_t hc = ((d & 0x03) << 4) | (this->flen & 0xC000) >> 12;

    this->check = d;
    if (hc != (this->calc & 0X3C)) {
      this->state = 0;
      return 0;
    }
    this->state = 4;
    this->flen &= 0x3FFF;
  } else if (this->state == 4) {
    this->data[this->cnt++] = d;
    this->calc ^= d;

    if (this->cnt == this->flen) {
      this->state = 0;

      //接收完，检查check
      if ((this->calc & 0xFC) == (this->check & 0XFC)) {
        return 1;
      } else {
        return 0;
      }
    }
  } else {
    this->state = 0;
  }
  return 0;
}

/**
 * @brief  封包数据并发送
 * @note
 * @param  *packer:
 * @param  *buff:
 * @param  size:
 * @retval None
 */
QByteArray Upacker::upacker_pack(QByteArray buff) {

  uint8_t crc = 0;
  uint8_t tmp[4] = {0};
  int size = buff.length();
  if (size > 16384) {
    return 0;
  }

  tmp[0] = STX_L;
  tmp[1] = size & 0xff;
  tmp[2] = (size >> 8) & 0x3f; //低14位用来保存size;header校验4位
  crc = tmp[0] ^ tmp[1] ^ tmp[2];
  tmp[2] |= (crc & 0x0C) << 4; // tmp[2][7:6]保存header检验[3:2]
  tmp[3] = 0x03 & (crc >> 4);  // tmp[3][1:0]保存header校验[5:4]
  for (int i = 0; i < size; i++) {
    crc ^= buff.at(i);
  }

  tmp[3] |= (crc & 0xfc); // tmp[3][7:2]保存data check[7:2]

  return QByteArray((const char *)tmp, 4).append(buff);
}
